/*
 *  init.cpp
 *  FaceTracker
 *
 *  Created by Kenn Sebesta on 2007-11-22.
 *  Copyright 2007 __MyCompanyName__. All rights reserved.
 *
 */

#include "bandpIncludes.h"
#include "createCamera.h"
#include "displayOpenCV.h"
#include "dLQRSolver.h"
#include "findBallState.h"
#include "init.h"
#include "maskGenerate.h"
#include "processImage.h"
#include "sendOutput.h"


const char  *WINDOW_INIT  = "Ball and Plate";
extern const char *cameraLib;
extern int viewSize;

void init() {
    //====================================================================================
    // Read from file
    //====================================================================================
    
    // Generate hash
    // GENERATE A HASH OF FILE SO WE CAN BE SURE THE DATA IS GOOD.
    
    double ballDia;		// Ball diameter in [m]
    double calibrationGuess;	// Calibration of camera image in [px/m]
    double delT; // Sampling rate in [s]
    SystemDims table;
    
    //Initialize Matrices
    CvMat *C, *Q_kalman, *R_kalman, *Q_dlqr, *R_dlqr;
    
    // Read user config file
    CvFileStorage* userVariables = cvOpenFileStorage( "bandpUserVariables.yml", 0, CV_STORAGE_READ);
    delT = cvReadRealByName( userVariables, NULL, "delT");
    table.actuator_1=(CvMat*)cvReadByName( userVariables, NULL, "actuator_1" );
    table.actuator_2=(CvMat*)cvReadByName( userVariables, NULL, "actuator_2" );
    Q_kalman=(CvMat*)cvReadByName( userVariables, NULL, "Q_kalman" );
    R_kalman=(CvMat*)cvReadByName( userVariables, NULL, "R_kalman" );
    Q_dlqr=(CvMat*)cvReadByName( userVariables, NULL, "Q_dlqr" );
    R_dlqr=(CvMat*)cvReadByName( userVariables, NULL, "R_dlqr" );
    ballDia=cvReadRealByName( userVariables, NULL, "ballDia");
    calibrationGuess=cvReadRealByName( userVariables, NULL, "calibrationGuess");
    
    // Stupidity check
    std::cout << "calibrationGuess is  " << calibrationGuess << "\n";
    printf("\nActuator 1="); printCvArr( table.actuator_1 );
    printf("\nActuator 2="); printCvArr( table.actuator_2 );
    printf("\nQ_kalman="); printCvArr( Q_kalman );
    printf("\nR_kalman="); printCvArr( R_kalman );
    printf("\nQ_dlqr="); printCvArr( Q_dlqr );
    printf("\nR_dlqr="); printCvArr( R_dlqr );
    
    // Local Garbage collection
    cvReleaseFileStorage( &userVariables );
    
    // RIGHT NOW, THERE IS NO FILE READING FOR THE BELOW.
    C=cvCreateMat(2, 4, CV_32F);	cvZero(C);	cvSetReal2D( C, 0, 0, 1 );	cvSetReal2D( C, 1, 2, 1);
    
    printf("\nDone with file read.\n\n");
    
    
    //==============================================
    // Open ports (both Signal Output and TCP Socket)
    //==============================================
    
    //Serial ports are OS dependent.  Might consider eventual migration to GNU Common C++, which is multiplatform
    int fd = openPort();
#ifdef SERIAL
    char szBuf[22];
    sprintf(szBuf, "#12 P1500 #28 P15000\r");
    
    if( int n = write(fd, szBuf, 22) )
        fputs("write() of 21 bytes failed!\n", stderr);
#endif
    
    //=====================================================================================
    // Initialize camera.
    //=====================================================================================
    CamHandle camFeed; //Structure that includes OpenCV or Camwire camera instructions
    IplImage *camFrame;
    if(!strcmp(cameraLib, "camwire"))
        camFrame=cvCreateImage( cvSize(480, 480), IPL_DEPTH_8U, 1 ); //NEEDS SIZE TO BE AUTOMATICALLY CONFIGURED. How?
    
    setupCamera( &camFeed );            //Sets up camera
    queryCamera( &camFrame, camFeed );  //Returns image
    
    if ( !camFrame ) {
        fprintf(stderr, "ERROR: queryCamera is returning null. Exiting...\n");
        exit(1);
    }
    
    printf( "Camera initialized.\n\n" );
    
    // Loads sample image (for when no camera is present)
//		IplImage* camFrame=cvLoadImage( "eightball.tiff", 3);
//		if(!camFrame) printf("Could not load image file.\n"), exit(1);
    
    
    //=====================================================================================
    // Camera adjustment. MESSSY SECTION. NEEDS TO BE IMPROVED.
    //=====================================================================================
    
    // Setup
    char wndname[] = "Live Edge Detection";
    char tbarname1[] = "Upper Threshold";
    char tbarname2[] = "Lower Threshold";
    
    ImageAnalysisParams params;
    
    // Binarization threshold
    IplImage *bwImage;
    bwImage=cvCreateImage( cvSize(camFrame->width, camFrame->height), camFrame->depth, 1 );
    IplImage *grayImage;
    grayImage=cvCreateImage( cvSize(camFrame->width, camFrame->height), camFrame->depth, 1 );
    cvNamedWindow(wndname, 1);
    ImageSet images;

    int binaryThresh=20;
    params.cannyThreshLow=30;
    params.cannyThreshHigh=50;
    // create a toolbar
    cvCreateTrackbar(tbarname1, wndname, &binaryThresh, 255, NULL );
    
    // Find best threshold value for binarization
    while(( cvWaitKey(10) & 255 ) != 27) {
        queryCamera( &camFrame, camFeed );
        if (camFrame->nChannels!=1){
            cvSplit( camFrame, grayImage, NULL, NULL, NULL );
        }
        else{
            cvCopy(camFrame, grayImage);
        }
        // Run the edge detector on grayscale
        cvThreshold(grayImage, bwImage, (double)binaryThresh, 255, CV_THRESH_BINARY );
        
        // Show the image
        cvShowImage(wndname, bwImage);
    }
    
    cvDestroyWindow( wndname );
    
    
    // Edge extraction thresholds
//    IplImage *edgeImage;
    images.edgeImage=cvCreateImage( cvSize(camFrame->width, camFrame->height), camFrame->depth, 1 );
    
    cvNamedWindow(wndname, CV_WINDOW_AUTOSIZE);
    
    // create a toolbar
    cvCreateTrackbar(tbarname1, wndname, &params.cannyThreshHigh, 100, NULL );
    cvCreateTrackbar(tbarname2, wndname, &params.cannyThreshLow, 100, NULL );
    
    // Find best threshold values for edge extraction
    while(( cvWaitKey(10) & 255 ) != 27) {
        queryCamera( &camFrame, camFeed );
        if (camFrame->nChannels!=1){
            cvSplit( camFrame, grayImage, NULL, NULL, NULL );
        }
        else{
            cvCopy(camFrame, grayImage);
        }
        // Run the edge detector on grayscale
        cvCanny(grayImage, images.edgeImage, (float)params.cannyThreshLow, (float)params.cannyThreshHigh, 3);
        
        // Show the image
        cvShowImage(wndname, images.edgeImage);
    }
    
    cvDestroyWindow( wndname );
    printf("\nDone setting up camera. \nThe binarization threshold value is %d. \nThe Canny threshold values are %d and %d.\n\n", binaryThresh, params.cannyThreshHigh, params.cannyThreshLow);
    
    
    //=====================================================================================
    // Auto-calibration and maskImage generation
    //=====================================================================================
    cvNamedWindow( WINDOW_INIT, CV_WINDOW_AUTOSIZE );
    double oldMaxConv=0, maxConv=0, peakConv=0, *convMax, bestCalibration=calibrationGuess;
    convMax=&maxConv; //Necessary to assign pointer, or else things crash.
    CvRect cropRect;
    cropRect.x=0;     cropRect.y=0;    cropRect.width=camFrame->width;    cropRect.height=camFrame->height;
    CvMat *y_measrd = cvCreateMat( 2, 1, CV_32F );
    IplImage *cropImage, *edgeImageOUT;
    CvMat *convMatOUT;
    CvMat* dft_mIm = cvCreateMat( 1, 1, CV_32F );
    viewSize=camFrame->width;
    
#if defined DEBUG && (DEBUG > 2)
    printf("Autocalibrating...\n");
#endif
    
    //Auto-calibration loop. PRETTY BOLLOXED UP. NEEDS TO BE RETHOUGHT.
    int count=0;
    int stopCount=200;
    int missingCount;
    double step=2; // Decreases calibration by "step" [px/m] by in each iteration.
    while(peakConv - maxConv < 2e06) {
        count++;
        missingCount=1;
        cropRect=cvRect( 0, 0, camFrame->width, camFrame->height );
#if defined DEBUG && (DEBUG > 2)
        printf("Error 30\n");
#endif
        if (count > stopCount){
            fprintf(stderr, "ERROR: cannot find proper calibration. Continuing...\n");
            break;
        }
        oldMaxConv=maxConv;
        calibrationGuess-=step;
        
        
        // Creates ball mask
        IplImage* maskImage;
        maskImage = maskGenerate( ballDia, calibrationGuess );
        cvShowImage( WINDOW_INIT, maskImage );
        cvWaitKey(10);
        
        ballFinder( maskImage, camFrame, images, &dft_mIm, cropRect, params, //Inputs
                &missingCount, NULL, convMax, y_measrd, &convMatOUT, &edgeImageOUT ); //Outputs
        
        // Tests fit
        if (peakConv < maxConv)
            peakConv=maxConv, bestCalibration = calibrationGuess;
        
        // Local garbage collection
        cvReleaseImage( &maskImage );
    }
//    bestCalibration=1030;
    // Show results
    IplImage* maskImage = maskGenerate( ballDia, bestCalibration );
    std::cout << "Best calibration value is " << bestCalibration << std::endl;
    cvCircle( camFrame, cvPoint(cvRound(cvmGet(y_measrd, 0, 0)), cvRound(cvmGet(y_measrd, 1, 0))), (maskImage->width-1)/2, cvScalar(255, 255, 255), 1 );
    cvShowImage( WINDOW_INIT, camFrame );
    cvWaitKey(1200);
    
    // Local garbage collection
    cvReleaseMat(	&convMatOUT );
    cvReleaseMat( &dft_mIm );
    cvReleaseImage( &edgeImageOUT );
    
    printf("Done with auto-calibration.\n\n");
    
    
    //====================================================================================
    // Discretized matrix Generation
    //====================================================================================
    CvMat *A, *B, *expNthPower, *expMatA, *expMatB, *expMat;
    CvMat *A_dscrt, *B_dscrt;
    double x1=cvmGet( table.actuator_1, 0, 0 );	double y1=cvmGet( table.actuator_1, 0, 1 );
    double x2=cvmGet( table.actuator_2, 0, 0 );	double y2=cvmGet( table.actuator_2, 0, 1 );
    double kt=5.0/7.0, g=9.81, Nz=x1*y2-x2*y1; // Physical constants
    
    // Initialize matrices
    A=cvCreateMat( 4, 4, CV_32F );
    B=cvCreateMat( 4, 2, CV_32F );
    A_dscrt=cvCreateMat(4, 4, CV_32F);
    B_dscrt=cvCreateMat(4, 2, CV_32F);
    expNthPower=cvCreateMat( A->width+B->width, A->height+B->width, CV_32F );
    expMat=cvCreateMat( A->width+B->width, A->height+B->width, CV_32F );
    expMatA=cvCreateMat( A->width, A->height, CV_32F );
    expMatB=cvCreateMat( B->width, B->height, CV_32F );
    
    // Fill matrices
    cvZero( A ); cvZero( B ); 	cvZero( expNthPower );
    cvmSet( A, 0, 1, 1); cvmSet( A, 2, 3, 1);
    cvmSet( B, 1, 0, kt*g/Nz*(-y2)); cvmSet( B, 1, 1, kt*g/Nz*(y1)); cvmSet( B, 3, 0, kt*g/Nz*(x2)); cvmSet( B, 3, 1, kt*g/Nz*(-x1));
    cvSetIdentity( expMat, cvScalar(1) );
    cvGetSubRect( expNthPower, expMatA, cvRect( 0, 0, A->width, A->height ) );
    cvGetSubRect( expNthPower, expMatB, cvRect( A->width, 0, B->width, B->height ) );
    cvCopy( A, expMatA );
    cvCopy( B, expMatB );
    cvScale( expNthPower, expNthPower, delT );
    
    // Calculate e^[expMat].
    count=0;
    cvAdd( expMat, expNthPower, expMat );
    while( cvNorm( expNthPower ) > 0.0001) {
        count++;
        if (count > 100){
            fprintf(stderr, "ERROR: Linear matrices do not converge to discrtized matrices...\n");
            exit(1);
        }
        cvMatMul( expNthPower, expNthPower, expNthPower );
        cvAddWeighted(expMat, 1, expNthPower, 1.0/(double)count, 0, expMat );
    }
    
    // Extract discretized matrices
    cvGetSubRect( expMat, expMatA, cvRect( 0, 0, A->width, A->height ) );
    cvGetSubRect( expMat, expMatB, cvRect( A->width, 0, B->width, B->height ) );
    cvCopy( expMatA, A_dscrt );
    cvCopy( expMatB, B_dscrt );
    
    printf("\nA_dscrt="); printCvArr(A_dscrt);
    printf("\nB_dscrt="); printCvArr(B_dscrt);
    printf("\nC="); printCvArr(C);
    
    //Local garbage collection
    cvReleaseMat( &A );
    cvReleaseMat( &B );
    cvReleaseMat( &expNthPower );
    cvReleaseMat( &expMat );
    cvReleaseMat( &expMatA );
    cvReleaseMat( &expMatB );
    
    printf("Done with matrix generation.\n");
    
    
    //====================================================================================
    // Initialize Kalman filter error covariance matrix.
    //====================================================================================
    CvKalman *kalman_dscrt;
    kalman_dscrt = cvCreateKalman(4, 2, 2);
    CvMat *oldPkCovMat;
    oldPkCovMat=cvCreateMat(4, 4, CV_32F);
    CvMat *y_measrdR = cvCreateMat( 2, 1, CV_32F );
    cvScale( y_measrd, y_measrdR, 1/bestCalibration );
    
    cvCopy(A_dscrt, kalman_dscrt->transition_matrix);						// A Matrix
    cvCopy(B_dscrt, kalman_dscrt->control_matrix);							// B Matrix
    cvCopy(C, kalman_dscrt->measurement_matrix);								// C Matrix
    cvCopy(Q_kalman, kalman_dscrt->process_noise_cov);						// Q Matrix
    cvCopy(R_kalman, kalman_dscrt->measurement_noise_cov);				// R Matrix
    
    
    // Repeats until the error covariance matrix converges.
    double diffNorm=1;
    while (  diffNorm> 0.000001 ){
        cvCopy( kalman_dscrt->error_cov_post, oldPkCovMat ); // Save old error covariance matrix
        cvKalmanPredict( kalman_dscrt, 0 );
        cvKalmanCorrect( kalman_dscrt, y_measrdR );
        diffNorm=cvNorm( oldPkCovMat, kalman_dscrt->error_cov_post ); // Are we converged yet?
    }
    
    std::cout << std::endl << "The error covariance matrix is:";	printCvArr( kalman_dscrt->error_cov_post );
    
    // Local garbage collection
    cvReleaseMat(&oldPkCovMat);
    
    printf("\nDone with Kalman filter initialization.\n");
    
    
    //=====================================================================================
    // Generate DLQR Solver
    //=====================================================================================
    CvMat *K_dlqr;
    K_dlqr=dLQRSolver( A_dscrt, B_dscrt, Q_dlqr, R_dlqr );
    std::cout << std::endl<< "The dLQR matrix is:";	printCvArr( K_dlqr );
    
    // Local garbage collection
    cvReleaseMat( &Q_dlqr );
    cvReleaseMat( &R_dlqr );
    
    printf("\nDone with DLQR Solver.\n\n");
    
    
    //=====================================================================================
    // Write matrices to file
    //=====================================================================================
    CvFileStorage* systemVariables = cvOpenFileStorage( "bandpSystemVariables.yml", 0, CV_STORAGE_WRITE);
    cvWriteInt( systemVariables, "binaryThresh", binaryThresh );
    cvWriteInt( systemVariables, "cannyThreshHigh", params.cannyThreshHigh );
    cvWriteInt( systemVariables, "cannyThreshLow", params.cannyThreshLow );
    cvWriteReal( systemVariables, "calibration", bestCalibration );
    cvWrite( systemVariables, "A_dscrt", A_dscrt );
    cvWrite( systemVariables, "B_dscrt", B_dscrt );
    cvWrite( systemVariables, "C", C );
    cvWrite( systemVariables, "x_hatR", kalman_dscrt->state_post );
    cvWrite( systemVariables, "PkCovMat", kalman_dscrt->error_cov_post );
    cvWrite( systemVariables, "K_dlqr", K_dlqr );
    cvWrite( systemVariables, "maskImage", maskImage );
    cvReleaseFileStorage( &systemVariables );
    
    printf("Done with file write.\n\n");
    
    
    //=====================================================================================
    // Garbage collection
    //=====================================================================================
    cvDestroyWindow( WINDOW_INIT );
    releaseCamera( camFeed );
    cvReleaseMat( &A_dscrt );
    cvReleaseMat( &B_dscrt );
    cvReleaseMat( &C );
    cvReleaseMat( &K_dlqr );
    cvReleaseMat( &y_measrd );
    cvReleaseMat( &y_measrdR );
    cvReleaseImage( &maskImage );
    cvReleaseImage( &camFrame );
    cvReleaseKalman( &kalman_dscrt );
    return;
}
